package pckg;

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;

import pckg.realRobot.RealRobot;

public class Generator {

	private static String file = "PPwSt.txt";
	private static final int numberOfSamples = 15000;
	
	private static double vMax = 1.5;
	private static double vMin = -1.5;
	private static int tMax = 1500;
	private static int tMin = 500;
	private static int stMax = 256;
	private static int stMin = 9;
	
	public static void testRandomSteps(){
		int min = Integer.MAX_VALUE;
		int max = Integer.MIN_VALUE;
		int mean = 0;
		Random rand = new Random();
		for (int i = 1; i < 100000; i++){
			
			int val = getRandomSteps(rand);
			if (val < min){
				min = val;
			}
			if (val > max){
				max = val;
			}
			mean += val;
		}
		mean /= 100000.0;
		System.out.println(mean);
	}
	
	public static void generateDirsWSt(RealRobot robot, int dirL, int dirR){
		long start = System.nanoTime();
		Random random = new Random();
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
			double vL = 0;
			double vR = 0;
			
			double cmdVL = 0;
			double cmdVR = 0;
			int cmdTime = 0;
			State startState = new State(new Vec(0,0),0,0,0);
			RobotCommand cmd = new RobotCommand(0,0,0,0,0);
			for (int i = 0; i < numberOfSamples; i++){
				robot.simpleReset();
			
				int prevStepsL = getRandomSteps(random);
				int prevStepsR = getRandomSteps(random);
				
				vL = RealLifeProperties.speedToV(prevStepsL)*dirL;
				vR = RealLifeProperties.speedToV(prevStepsR)*dirR;
				
				if (prevStepsL > 255){
					vL = 0;
					prevStepsL = 255;
				}
				if (prevStepsR > 255){
					vR = 0;
					prevStepsR = 255;
				}
				
				int stepsL = getRandomSteps(random);
				int stepsR = getRandomSteps(random);
				
				cmdVL = RealLifeProperties.speedToV(stepsL)*dirL;
				cmdVR = RealLifeProperties.speedToV(stepsR)*dirR;
				cmdTime = getRandomTime(random);
			
				cmd.newCommand(cmdVL, 
						cmdVR, 
						cmdTime,
						prevStepsL,
						prevStepsR);
				
				startState.pos.x = 0;
				startState.pos.y = 0;
				startState.fi = 0;
				startState.vL = vL;
				startState.vR = vR;
				
				robot.runCmd(startState, cmd, null, true);
				
				double x = robot.getX();
				double y = robot.getY();
				double orient = robot.getOrient();
				
				//out.write(normV(vL) + " " + normV(vR) + " " + x + " " + y + " " + orient + " " + normV(vLend) + " " + normV(vRend) + " " + normV(cmdVL) + " " + normV(cmdVR) + " " + normT(cmdTime));
				out.write(	normSp(prevStepsL) + " " + normSp(prevStepsR) + " " 
							+ x + " " + y + " " + orient + " " 
							+ normSp(stepsL) + " " + normSp(stepsR) + " " 
							+ normT(cmdTime));
				out.newLine();
				
				if (i%1000 == 0){
					System.out.println("done: " + i + "/" + numberOfSamples);
				}
			}
			out.close();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		long end = System.nanoTime();
		robot.reset();
		System.out.println((end-start)/RealLifeProperties.s2ns/numberOfSamples);
	}
	
	
	public static void generate(RealRobot robot){
		long start = System.nanoTime();
		Random random = new Random();
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
			int dirL = 1;
			int dirR = 1;
			double vL = 0;
			double vR = 0;
			
			double cmdVL = 0;
			double cmdVR = 0;
			int cmdTime = 0;
			State startState = new State(new Vec(0,0),0,0,0);
			RobotCommand cmd = new RobotCommand(0,0,0,0,0);
			for (int i = 0; i < numberOfSamples; i++){
				robot.simpleReset();
				
				// dirL = getRandomDir(random);
				// dirR = getRandomDir(random);
				vL = getRandomSpeed(random)*dirL;
				vR = getRandomSpeed(random)*dirR;
				
				int prevStepsL = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vL)/2.0));
				int prevStepsR = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vR)/2.0));
				
				if (0.000245*115200*2/255.0 > vL){
					vL = 0;
					prevStepsL = 255;
				}
				if (0.000245*115200*2/255.0 > vR){
					vR = 0;
					prevStepsR = 255;
				}
				
				cmdVL = getRandomSpeed(random)*dirL;
				cmdVR = getRandomSpeed(random)*dirR;
				cmdTime = getRandomTime(random);
				
				cmd.newCommand(cmdVL, 
						cmdVR, 
						cmdTime,
						prevStepsL,
						prevStepsR);
				
				startState.pos.x = 0;
				startState.pos.y = 0;
				startState.fi = 0;
				startState.vL = vL;
				startState.vR = vR;
				
				robot.runCmd(startState, cmd, null, true);
				
				double x = robot.getX();
				double y = robot.getY();
				double orient = robot.getOrient();
				
				//out.write(normV(vL) + " " + normV(vR) + " " + x + " " + y + " " + orient + " " + normV(vLend) + " " + normV(vRend) + " " + normV(cmdVL) + " " + normV(cmdVR) + " " + normT(cmdTime));
				out.write(vR + " " + vL + " " + x + " " + y + " " + orient + " " + cmdVL + " " + cmdVR + " " + normT(cmdTime));
				out.newLine();
				
				if (i%1000 == 0){
					System.out.println("done: " + i + "/" + numberOfSamples);
				}
			}
			out.close();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		long end = System.nanoTime();
		robot.reset();
		System.out.println((end-start)/RealLifeProperties.s2ns/numberOfSamples);
	}
	
	public static void generateOneSample(RealRobot robot){
		long start = System.nanoTime();
		Random random = new Random();
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
			int dirL = 1;
			int dirR = 1;
			double vL = 0;
			double vR = 0;
			
			double cmdVL = 0;
			double cmdVR = 0;
			int cmdTime = 0;
			State startState = new State(new Vec(0,0),0,0,0);
			RobotCommand cmd = new RobotCommand(0,0,0,0,0);
			//for (int i = 0; i < numberOfSamples; i++){
				robot.simpleReset();
				
				// dirL = getRandomDir(random);
				// dirR = getRandomDir(random);
				//vL = 0.5867*dirL;
				//vR = 0.0036*dirR;
				vL = 0.0036*dirR;
				vR = 0.5867*dirL;
				int prevStepsL = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vL)/2.0));
				int prevStepsR = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vR)/2.0));
				
				if (0.000245*115200/2/255.0 > vL){
					vL = 0;
					prevStepsL = 255;
				}
				if (0.000245*115200/2/255.0 > vR){
					vR = 0;
					prevStepsR = 255;
				}
				
				cmdVL = 0.8166*dirL;
				cmdVR = 0.5730*dirR;
				cmdTime = 535;
				
				cmd.newCommand(cmdVL, 
						cmdVR, 
						cmdTime,
						prevStepsL,
						prevStepsR);
				
				startState.pos.x = 0;
				startState.pos.y = 0;
				startState.fi = 0;
				startState.vL = vL;
				startState.vR = vR;
				
				robot.runCmd(startState, cmd, null, true);
				
				double x = robot.getX();
				double y = robot.getY();
				double orient = robot.getOrient();
				
				//out.write(normV(vL) + " " + normV(vR) + " " + x + " " + y + " " + orient + " " + normV(vLend) + " " + normV(vRend) + " " + normV(cmdVL) + " " + normV(cmdVR) + " " + normT(cmdTime));
				//out.write(vL + " " + vR + " " + x + " " + y + " " + orient + " " + cmdVL + " " + cmdVR + " " + normT(cmdTime));
				//out.newLine();
				
			
			//}
				robot.simpleReset();
			out.close();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		long end = System.nanoTime();
		robot.reset();
		System.out.println((end-start)/RealLifeProperties.s2ns/numberOfSamples);
	}
	
	public static void generateAllDirs(RealRobot robot){
		file = "PN.txt";
		generateDirs(robot,+1,-1);
		System.out.println("PN Done\n");
		file = "NP.txt";
		generateDirs(robot,-1,+1);
		System.out.println("NP Done\n");
		file = "NN.txt";
		generateDirs(robot,-1,-1);
		System.out.println("NN Done\n");
	}
	
	public static void generateDirs(RealRobot robot, int dirL, int dirR){
		long start = System.nanoTime();
		Random random = new Random();
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
			double vL = 0;
			double vR = 0;
			
			double cmdVL = 0;
			double cmdVR = 0;
			int cmdTime = 0;
			State startState = new State(new Vec(0,0),0,0,0);
			RobotCommand cmd = new RobotCommand(0,0,0,0,0);
			for (int i = 0; i < numberOfSamples; i++){
				robot.simpleReset();
				
				vL = getRandomSpeed(random)*dirL;
				vR = getRandomSpeed(random)*dirR;
			
				int prevStepsL = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vL)/2.0));
				int prevStepsR = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vR)/2.0));
				
				if (0.000245*115200/2/255.0 > Math.abs(vL)){
					vL = 0;
					prevStepsL = 255;
				}
				if (0.000245*115200/2/255.0 > Math.abs(vR)){
					vR = 0;
					prevStepsR = 255;
				}
				
				cmdVL = getRandomSpeed(random)*dirL;
				cmdVR = getRandomSpeed(random)*dirR;
				cmdTime = getRandomTime(random);
			
				cmd.newCommand(cmdVL, 
						cmdVR, 
						cmdTime,
						prevStepsL,
						prevStepsR);
				
				startState.pos.x = 0;
				startState.pos.y = 0;
				startState.fi = 0;
				startState.vL = vL;
				startState.vR = vR;
				
				robot.runCmd(startState, cmd, null, true);
				
				double x = robot.getX();
				double y = robot.getY();
				double orient = robot.getOrient();
				
				//out.write(normV(vL) + " " + normV(vR) + " " + x + " " + y + " " + orient + " " + normV(vLend) + " " + normV(vRend) + " " + normV(cmdVL) + " " + normV(cmdVR) + " " + normT(cmdTime));
				out.write(vL + " " + vR + " " + x + " " + y + " " + orient + " " + cmdVL + " " + cmdVR + " " + normT(cmdTime));
				out.newLine();
				
				if (i%1000 == 0){
					System.out.println("done: " + i + "/" + numberOfSamples);
				}
			}
			out.close();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		long end = System.nanoTime();
		robot.reset();
		System.out.println((end-start)/RealLifeProperties.s2ns/numberOfSamples);
	}
	
	
	public static void generate2(RealRobot robot){
		//long start = System.nanoTime();
		
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
		
			
			State startState = new State(new Vec(0,0),0,0,0);
			RobotCommand cmd = new RobotCommand(0,0,0,0,0);
			for (double vLStart = -1.5; vLStart <= 1.5; vLStart += 0.25){
				System.out.println("vLStart : " + vLStart);
				for (double vRStart = -1.5; vRStart <= 1.5; vRStart += 0.25){
					System.out.println("vRStart : " + vRStart);
					for (double cmdVL = -1.5; cmdVL <= 1.5; cmdVL += 0.25){
						for (double cmdVR = -1.5; cmdVR <= 1.5; cmdVR += 0.25){
							for (int cmdTime = 500; cmdTime <= 5000; cmdTime += 500){
								
								if (vLStart*cmdVL < 0 || vRStart*cmdVR < 0 ){
									continue;
								}
								
								
								robot.simpleReset();
								
								cmd.newCommand(cmdVL, 
										cmdVR, 
										cmdTime,
										(int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vLStart)/2.0)),
										(int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/Math.abs(vRStart)/2.0)));
								
								startState.pos.x = 0;
								startState.pos.y = 0;
								startState.fi = 0;
								startState.vL = vLStart;
								startState.vR = vRStart;
								
								robot.runCmd(startState, cmd, null, true);
								
								double x = robot.getX();
								double y = robot.getY();
								double orient = robot.getOrient();
								
								out.write(vLStart + " " + vRStart + " " + x + " " + y + " " + orient + " " + cmdVL + " " + cmdVR + " " + normT(cmdTime));
								out.newLine();
								
								
							}
						}
					}
				}
			}
			
			out.close();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		//long end = System.nanoTime();
		robot.reset();
		//System.out.println((end-start)/RealLifeProperties.s2ns/numberOfSamples);
	}
	
	public static double normT(int t){
		return ((double)(t-tMin))/(tMax-tMin);
	}
	
	private static int getRandomTime(Random rand){
		return rand.nextInt(tMax-tMin)+tMin;
	}
	
	private static double normV(double v){
		return (v-vMin)/(vMax-vMin);
	}
	
	private static int getRandomSteps(Random rand){
		double min = 1.0/(stMax);
		double max = 1.0/stMin;
		
		return (int)Math.round(1.0/(rand.nextDouble()*(max-min) + min));
		/*
		double speed = getRandomSpeed(rand);
		int val = (int)Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/speed/2.0));
		
		if (speed < 0.055){
			val = 256;
		}
		
		return val;
		*/
	}
	
	public static double normSp(int speed){
		return ((double)(speed-stMin))/(stMax-stMin);
	}
	
	public static int denormSt(double normSp){
		return (int)Math.round(normSp*(stMax-stMin)+stMin);
	}
	
	private static double getRandomSpeed(Random rand){
		
		double val = rand.nextDouble()*1.5;
		//return rand.nextDouble()*(1.5-0.06)+0.06;
		
		if (val < 0.000245*115200/2/255.0){
			return 0;
		} else {
			return val;
		}
		
	}
	private static int getRandomDir(Random rand){
		return rand.nextDouble() > 0.5 ? 1 : -1;
	}
	
}
